////////////////////////////////////////////////////////////////////////
//HEADERS
////////////////////////////////////////////////////////////////////////
#include<stdio.h>
#include<stdlib.h>  
#include<string.h>  
#include<malloc.h>
#include<math.h>
#include<time.h>

//CABECERAS DE GSL
#include<gsl/gsl_math.h>
#include<gsl/gsl_errno.h>
#include<gsl/gsl_sort.h>
#include<gsl/gsl_sort_vector.h>
#include<gsl/gsl_matrix.h>
#include<gsl/gsl_spline.h>
//Minimizacion
#include <gsl/gsl_min.h>
#include <gsl/gsl_multimin.h>
#include<gsl/gsl_bspline.h>
#include <gsl/gsl_multifit.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_deriv.h>

typedef struct
{
  int snap;
  int id;
  float cm[3];
  float vcm[3];
  float lcm[3];
  float M;
  float z;
} CMS;

int contadorParticulas(char *infile) 
{

  int NDAT,c;
  static FILE *pf;

  if((pf=fopen(infile,"r"))==NULL)
    printf("no puedo abrir archivo 1\n");

  NDAT=0;

  while((c=fgetc(pf))!= EOF)
    {
      if(c == '\n')
        {
          ++NDAT;
        }
    }

  fclose(pf);

  return NDAT;

}

int centerMassPart(CMS *part, CMS *centerm, int ini, int fin)
{
  int i;
  double mtot=0.0,cm[3],vcm[3],lcm[3];

  for(i=0; i<3; i++)
    {
      cm[i]=0.0;
      vcm[i]=0.0;
      lcm[i]=0.0;
    }

  for(i=ini; i<fin; i++)
    {
      printf("%d - %d %d %d\n",ini,fin,i,part[i].id);
      cm[0] = cm[0] + part[i].cm[0]*part[i].M;
      cm[1] = cm[1] + part[i].cm[1]*part[i].M;
      cm[2] = cm[2] + part[i].cm[2]*part[i].M;
      
      vcm[0] = vcm[0] + part[i].vcm[0]*part[i].M;
      vcm[1] = vcm[1] + part[i].vcm[1]*part[i].M;
      vcm[2] = vcm[2] + part[i].vcm[2]*part[i].M;
      
      lcm[0] = lcm[0] + (part[i].cm[1]*part[i].vcm[2] 
			 - part[i].cm[2]*part[i].vcm[1])*part[i].M;
      lcm[1] = lcm[1] - (part[i].cm[0]*part[i].vcm[2] 
			 - part[i].cm[2]*part[i].vcm[0])*part[i].M;
      lcm[2] = lcm[2] + (part[i].cm[0]*part[i].vcm[1] 
			 - part[i].cm[1]*part[i].vcm[0])*part[i].M;
      
      mtot = mtot + part[i].M;
    }
 
  (*centerm).cm[0] = cm[0]/mtot;
  (*centerm).cm[1] = cm[1]/mtot;
  (*centerm).cm[2] = cm[2]/mtot;
  
  (*centerm).vcm[0] = vcm[0]/mtot;
  (*centerm).vcm[1] = vcm[1]/mtot;
  (*centerm).vcm[2] = vcm[2]/mtot;
  
  (*centerm).lcm[0] = lcm[0];
  (*centerm).lcm[1] = lcm[1];
  (*centerm).lcm[2] = lcm[2];

  (*centerm).M = mtot;
 
  return 0;
}

int main(int argc, char *argv[])
{
  int i,j,k,nsat,nz=0,liminf,limup;
  float facscala;
  CMS *satellites,cmsystem;
  char *infile;
  FILE *data;

  infile = argv[1];
 
  data = fopen(infile,"r");
  nsat = contadorParticulas(infile);

  int nhalos[nsat];
  float z[nsat];
  satellites = (CMS *)malloc((size_t)nsat*sizeof(CMS));

  for(i=0; i<nsat; i++)
    {
      fscanf(data,"%d %d %e %e %e %e %e %e %e %e",
	     &satellites[i].snap,
	     &satellites[i].id,
	     &satellites[i].cm[0],&satellites[i].cm[1],&satellites[i].cm[2],
	     &satellites[i].vcm[0],&satellites[i].vcm[1],&satellites[i].vcm[2],
	     &satellites[i].M,
	     &satellites[i].z);
      /* printf("%d %f %f %f %f %f %f %f %f\n",
	     satellites[i].id,
	     satellites[i].cm[0],satellites[i].cm[1],satellites[i].cm[2],
	     satellites[i].vcm[0],satellites[i].vcm[1],satellites[i].vcm[2],
	     satellites[i].M,satellites[i].z);*/
      facscala = 1.0/(1.0+satellites[i].z);
      satellites[i].cm[0] = satellites[i].cm[0]*1000*facscala;
      satellites[i].cm[1] = satellites[i].cm[1]*1000*facscala;
      satellites[i].cm[2] = satellites[i].cm[2]*1000*facscala;
      nhalos[i] = 1;
      z[i] = 0.0;
    }
  
  k=0;
  for(i=1; i<nsat; i++)
    {
      if((satellites[i-1].z-satellites[i].z) <(1e-6))
	{
	  z[k] = satellites[i].z;
	  nhalos[k] =nhalos[k]+1;
	  printf("%e\t",(satellites[i-1].z)-(satellites[i].z));
	  printf("%lf %d  %d\n",z[k],k,nhalos[k]);
	}
      else
	{
	  printf("no iguales\n");
	  //nhalos[k] =nhalos[k]+1;
	  k = k+1;
	  if(i==nsat-1)
	    { 
	      z[k] = satellites[i].z;
	      //nhalos[k] = nhalos[k]+1;
	      nz = k+1;
	      printf("%lf %d  %d nz : %d\n",z[k],k,nhalos[k],nz);	     
	    }
	}
    }

 for(i=0; i<nz; i++)
   {
     printf("%d %d\n",i,nhalos[i]);
   }
 
  FILE *L;
  L = fopen("orientations_WH.dat","w"); 
  liminf = 0;
  k = 0; 
  for(i=0; i<nz; i++)
    {
      limup = liminf+nhalos[i];
      //CMS *cmsystem;
      //cmsystem = (CMS *)malloc((size_t)nhalos[i]*sizeof(CMS));
      centerMassPart(satellites,&cmsystem,liminf,limup);

      printf("\nini: %d - fin : %d with %d halos\n",liminf,limup,nhalos[i]);

      printf("\ncenter of mass of the system:\n");
      printf("CMS : %e %e %e\n",cmsystem.cm[0],cmsystem.cm[1],cmsystem.cm[2]);
      printf("VCMS : %e %e %e\n",cmsystem.vcm[0],cmsystem.vcm[1],cmsystem.vcm[2]);
      printf("LCMS : %e %e %e\n",cmsystem.lcm[0],cmsystem.lcm[1],cmsystem.lcm[2]);
      printf("M total : %e\n",cmsystem.M);
     
      for(k=liminf; k<limup; k++)
	{
	  /*
	  for(j=0; j<3; j++)
	    {
	      satellites[k].cm[j] = satellites[k].cm[j]-cmsystem.cm[j]; 
	      satellites[k].vcm[j] = satellites[k].vcm[j]-cmsystem.vcm[j]; 
	      //printf("\n%e %e\n",satellites[k].cm[j],satellites[k].vcm[j]);
	      }*/
	  for(j=0; j<3; j++)
	    {
	      satellites[k].cm[j] = satellites[k].cm[j]-satellites[liminf].cm[j]; 
	      satellites[k].vcm[j] = satellites[k].vcm[j]-satellites[liminf].cm[j]; 
	      //printf("\n%e %e\n",satellites[k].cm[j],satellites[k].vcm[j]);
	    }
	}

      centerMassPart(satellites,&cmsystem,liminf,limup);

      printf("\ncenter of mass of the system:\n");
      printf("NEW CMS : %e %e %e\n",cmsystem.cm[0],cmsystem.cm[1],cmsystem.cm[2]);
      printf("NEW VCMS : %e %e %e\n",cmsystem.vcm[0],cmsystem.vcm[1],cmsystem.vcm[2]);
      printf("NEW LCMS : %e %e %e\n",cmsystem.lcm[0],cmsystem.lcm[1],cmsystem.lcm[2]);
      printf("NEW M total : %e\n",cmsystem.M);

      fprintf(L,"%.8f %e %e %e %e %d\n",z[i],cmsystem.lcm[0],cmsystem.lcm[1],cmsystem.lcm[2],cmsystem.M,limup-liminf);

      liminf = limup;
    }
  
  fclose(L);

  printf("#######################\n");
  printf("\n\nONLY SATELLITES\n\n");
  printf("#######################\n");

  FILE *L1,*L2;
  L1 = fopen("orientations.dat","w"); 
  L2 = fopen("orientations_host.dat","w"); 
  liminf = 0;
  k = 0; 
  for(i=0; i<nz; i++)
    {
      limup = liminf+nhalos[i];
      //CMS *cmsystem;
      //cmsystem = (CMS *)malloc((size_t)nhalos[i]*sizeof(CMS));
      centerMassPart(satellites,&cmsystem,liminf+1,limup);

      printf("\nini: %d - fin : %d with %d halos\n",liminf+1,limup,nhalos[i]-1);

      printf("\ncenter of mass of the system:\n");
      printf("CMS : %e %e %e\n",cmsystem.cm[0],cmsystem.cm[1],cmsystem.cm[2]);
      printf("VCMS : %e %e %e\n",cmsystem.vcm[0],cmsystem.vcm[1],cmsystem.vcm[2]);
      printf("LCMS : %e %e %e\n",cmsystem.lcm[0],cmsystem.lcm[1],cmsystem.lcm[2]);
      printf("M total : %e\n",cmsystem.M);
 

      satellites[liminf].lcm[0] = (satellites[liminf].cm[1]*satellites[liminf].vcm[2] 
				   - satellites[liminf].cm[2]*satellites[liminf].vcm[1])*satellites[liminf].M;
      satellites[liminf].lcm[1] = (satellites[liminf].cm[0]*satellites[liminf].vcm[2] 
				   - satellites[liminf].cm[2]*satellites[liminf].vcm[0])*satellites[liminf].M;
      satellites[liminf].lcm[2] = (satellites[liminf].cm[0]*satellites[liminf].vcm[1] 
				   - satellites[liminf].cm[1]*satellites[liminf].vcm[0])*satellites[liminf].M;
      fprintf(L2,"%.8f %e %e %e %e %d\n",z[liminf],
	      satellites[liminf].lcm[0],satellites[liminf].lcm[1],satellites[liminf].lcm[2],
	      satellites[liminf].M,
	      liminf);
    
      for(k=liminf+1; k<limup; k++)
	{/*
	  for(j=0; j<3; j++)
	    {
	      satellites[k].cm[j] = satellites[k].cm[j]-cmsystem.cm[j]; 
	      satellites[k].vcm[j] = satellites[k].vcm[j]-cmsystem.vcm[j]; 
	      // printf("\n%e %e\n",satellites[k].cm[j],satellites[k].vcm[j]);
	      }*/
	  for(j=0; j<3; j++)
	    {
	      satellites[k].cm[j] = satellites[k].cm[j]-satellites[liminf].cm[j]; 
	      satellites[k].vcm[j] = satellites[k].vcm[j]-satellites[liminf].cm[j]; 
	      // printf("\n%e %e\n",satellites[k].cm[j],satellites[k].vcm[j]);
	    }
	}

      centerMassPart(satellites,&cmsystem,liminf+1,limup);

      printf("\ncenter of mass of the system:\n");
      printf("NEW CMS : %e %e %e\n",cmsystem.cm[0],cmsystem.cm[1],cmsystem.cm[2]);
      printf("NEW VCMS : %e %e %e\n",cmsystem.vcm[0],cmsystem.vcm[1],cmsystem.vcm[2]);
      printf("NEW LCMS : %e %e %e\n",cmsystem.lcm[0],cmsystem.lcm[1],cmsystem.lcm[2]);
      printf("NEW M total : %e\n",cmsystem.M);

      fprintf(L1,"%.8f %e %e %e %e %d\n",z[i],cmsystem.lcm[0],cmsystem.lcm[1],cmsystem.lcm[2],cmsystem.M,limup-liminf-1);
     
 
      liminf = limup;
    }
  
  fclose(L1);
  fclose(L2);

  /*
  phi = atan2(cmsystem.lcm[1],cmsystem.lcm[0])*180.0/M_PI;
  theta = atan2(cmsystem.lcm[2],cmsystem.lcm[0])*180.0/M_PI;

  if(phi>=0.0)
    {
      phi = phi;
    }
  else
    {
    phi = phi + 360.0;
    }
  if(theta>=0.0)
    {
      theta = theta;
    }
  else
    {
    theta = theta + 360.0;
    } 

  alpha = phi;
  beta = theta - 90.0; 

  gamma=0.0;
  delta=0.0;

  printf("alpha,beta,gamma,delta\n");
  printf("%lf %lf %lf %lf\n\n",alpha,beta,gamma,delta);
  */
  fclose(data);
  free(satellites);

  return 0;
}
